// agmatthews USERHOOKS
#include <JETI_EX_SENSOR.h>
#include <avr/pgmspace.h>

uint8_t jetiGpsLatitude[4] = {
  'N',0,0,0};
uint8_t jetiGpsLongitude[4] = {
  'E',0,0,0};
short jetiAltitude = 0;
short jetiNumSats = 0;
float jetiBoussoleHeading;
short jetiBoussole = 0;
short jetiHauteur = 0;
//uint8_t jetiDate[3] = {0,0,0};
//uint8_t jetiTime[3] = {0,0,0};
float jetiVoltage = 0;
float jetiCourant = 0;
float jetiPuissance = 0;
uint8_t jetiGpsLatitudeDataNb = 0;
uint8_t jetiGpsLongitudeDataNb = 0;
uint8_t jetiGpsAltitudeDataNb = 0;
uint8_t jetiWaypointDataNb = 0;
short jetiWpDistance=0;
short jetiWaypointNum = 0;

uint8_t jetiSelect = 0;
uint8_t jetiButton = 0;
#define JETI_START 0

#define JETI_GPS1 1
#define JETI_GPS2 2
#define JETI_BATTERY 3 

#define JETI_TEST 4

#define JETI_STB_MEMORY 10
const char jetiApParamName_10[] PROGMEM = "Stabilization";
const char jetiApParamName_11[] PROGMEM = "STB_RLL_P";
const char jetiApParamName_12[] PROGMEM = "STB_RLL_I";
//const char jetiApParamName_13[] PROGMEM = "STB_RLL_IMAX";
const char jetiApParamName_13[] PROGMEM = "STB_PIT_P";
const char jetiApParamName_14[] PROGMEM = "STB_PIT_I";
//const char jetiApParamName_16[] PROGMEM = "STB_PIT_IMAX";
const char jetiApParamName_15[] PROGMEM = "STB_YAW_P";
const char jetiApParamName_16[] PROGMEM = "STB_YAW_I";
//const char jetiApParamName_19[] PROGMEM = "STB_YAW_IMAX";
#define JETI_RATE_MEMORY 17
const char jetiApParamName_17[] PROGMEM = "Rate";
const char jetiApParamName_18[] PROGMEM = "RATE_RLL_P";
//const char jetiApParamName_22[] PROGMEM = "RATE_RLL_I";
//const char jetiApParamName_23[] PROGMEM = "RATE_RLL_D";
//const char jetiApParamName_24[] PROGMEM = "RATE_RLL_IMAX";
const char jetiApParamName_19[] PROGMEM = "RATE_PIT_P";
//const char jetiApParamName_26[] PROGMEM = "RATE_PIT_I";
//const char jetiApParamName_27[] PROGMEM = "RATE_PIT_D";
//const char jetiApParamName_28[] PROGMEM = "RATE_PIT_IMAX";
const char jetiApParamName_20[] PROGMEM = "RATE_YAW_P";
//const char jetiApParamName_30[] PROGMEM = "RATE_YAW_I";
//const char jetiApParamName_31[] PROGMEM = "RATE_YAW_D";
//const char jetiApParamName_32[] PROGMEM = "RATE_YAW_IMAX";
#define JETI_ALT_HOLD_MEMORY 21
const char jetiApParamName_21[] PROGMEM = "Alt Hold";
const char jetiApParamName_22[] PROGMEM = "THR_ALT_P";
//const char jetiApParamName_35[] PROGMEM = "THR_ALT_I";
//const char jetiApParamName_36[] PROGMEM = "THR_ALT_IMAX";
const char jetiApParamName_23[] PROGMEM = "THR_RATE_P";
//const char jetiApParamName_38[] PROGMEM = "THR_RATE_I";
//const char jetiApParamName_39[] PROGMEM = "THR_RATE_D";
//const char jetiApParamName_40[] PROGMEM = "THR_RATE_IMAX";
#define JETI_LOITER_MEMORY 24
const char jetiApParamName_24[] PROGMEM = "Loiter";
//const char jetiApParamName_42[] PROGMEM = "HLD_LAT_P";
//const char jetiApParamName_43[] PROGMEM = "HLD_LAT_I";
//const char jetiApParamName_44[] PROGMEM = "HLD_LAT_IMAX";
const char jetiApParamName_25[] PROGMEM = "HLD_LON_P";
//const char jetiApParamName_46[] PROGMEM = "HLD_LON_I";
//const char jetiApParamName_47[] PROGMEM = "HLD_LON_IMAX";
const char jetiApParamName_26[] PROGMEM = "LOITER_LAT_P";
//const char jetiApParamName_49[] PROGMEM = "LOITER_LAT_I";
//const char jetiApParamName_50[] PROGMEM = "LOITER_LAT_D";
const char jetiApParamName_27[] PROGMEM = "LOITER_LAT_IMAX";
//const char jetiApParamName_52[] PROGMEM = "LOITER_LON_P";
//const char jetiApParamName_53[] PROGMEM = "LOITER_LON_I";
//const char jetiApParamName_54[] PROGMEM = "LOITER_LON_D";
//const char jetiApParamName_55[] PROGMEM = "LOITER_LON_IMAX";
#define JETI_NAV_MEMORY 28
const char jetiApParamName_28[] PROGMEM = "Navigation";
const char jetiApParamName_29[] PROGMEM = "NAV_LAT_P";
//const char jetiApParamName_58[] PROGMEM = "NAV_LAT_D";
//const char jetiApParamName_59[] PROGMEM = "NAV_LAT_I";
//const char jetiApParamName_60[] PROGMEM = "NAV_LAT_IMAX";
const char jetiApParamName_30[] PROGMEM = "NAV_LON_P";
//const char jetiApParamName_62[] PROGMEM = "NAV_LON_D";
//const char jetiApParamName_63[] PROGMEM = "NAV_LON_I";
//const char jetiApParamName_64[] PROGMEM = "NAV_LON_IMAX";
const char jetiApParamName_31[] PROGMEM = "WP_SPEED_MAX";
#define JETI_GIMBAL_MEMORY 32
const char jetiApParamName_32[] PROGMEM = "Gimbal";
const char jetiApParamName_33[] PROGMEM = "MNT_MODE";
const char jetiApParamName_34[] PROGMEM = "MNT_STAB_PITCH";
const char jetiApParamName_35[] PROGMEM = "MNT_STAB_ROLL";
const char jetiApParamName_36[] PROGMEM = "MNT_STAB_YAW";
//const char jetiApParamName_70[] PROGMEM = "Mount";
const char jetiApParamName_37[] PROGMEM = "CAM_P_FUNCTION";
//const char jetiApParamName_72[] PROGMEM = "CAM_P_ANGLE_MAX";
//const char jetiApParamName_73[] PROGMEM = "CAM_P_ANGLE_MIN";
//const char jetiApParamName_74[] PROGMEM = "CAM_P_MAX";
//const char jetiApParamName_75[] PROGMEM = "CAM_P_TRIM";
//const char jetiApParamName_76[] PROGMEM = "CAM_P_MIN";
//const char jetiApParamName_77[] PROGMEM = "CAM_P_REV";
const char jetiApParamName_38[] PROGMEM = "CAM_R_FUNCTION";
//const char jetiApParamName_79[] PROGMEM = "CAM_R_ANGLE_MAX";
//const char jetiApParamName_80[] PROGMEM = "CAM_R_ANGLE_MIN";
//const char jetiApParamName_81[] PROGMEM = "CAM_R_MAX";
//const char jetiApParamName_82[] PROGMEM = "CAM_R_TRIM";
//const char jetiApParamName_83[] PROGMEM = "CAM_R_MIN";
//const char jetiApParamName_84[] PROGMEM = "CAM_R_REV";
const char jetiApParamName_39[] PROGMEM = "CAM_Y_FUNCTION";
//const char jetiApParamName_86[] PROGMEM = "CAM_Y_ANGLE_MAX";
//const char jetiApParamName_87[] PROGMEM = "CAM_Y_ANGLE_MIN";
//const char jetiApParamName_88[] PROGMEM = "CAM_Y_MAX";
//const char jetiApParamName_89[] PROGMEM = "CAM_Y_TRIM";
//const char jetiApParamName_90[] PROGMEM = "CAM_Y_MIN";
//const char jetiApParamName_91[] PROGMEM = "CAM_Y_REV";

#define JETI_LAST_PARAM 39

const char* jetiApParamName[] = {
  jetiApParamName_10, jetiApParamName_11, jetiApParamName_12, jetiApParamName_13, jetiApParamName_14, jetiApParamName_15, jetiApParamName_16, jetiApParamName_17, jetiApParamName_18, jetiApParamName_19,
  jetiApParamName_20, jetiApParamName_21, jetiApParamName_22, jetiApParamName_23, jetiApParamName_24, jetiApParamName_25, jetiApParamName_26, jetiApParamName_27, jetiApParamName_28, jetiApParamName_29,
  jetiApParamName_30, jetiApParamName_31, jetiApParamName_32, jetiApParamName_33, jetiApParamName_34, jetiApParamName_35, jetiApParamName_36, jetiApParamName_37, jetiApParamName_38
};

AP_Param *jetiAp;
enum ap_var_type jetiApType;

uint8_t jetiMenu = JETI_START;
char jetiBufferMsg[0x12];
char jetiBufferMsg2[0x12];
String stringBuffer1;
String stringBuffer2;
uint16_t gpsMinute = 0;

char testChar = 0;
bool lastMotorArmed=false;
uint16_t beforeRepeatAlarm = 0; 
GPS::GPS_Status jetiLastGpsStatus = GPS::NO_GPS;

static int8_t jetiLaseControlMode = STABILIZE;


void userhook_init()
{
  // put your initialisation code here
  JB.JetiBox(THISFIRMWARE,"     STATUS");
  JB.UartInit("ArduCopter");
  jetiGpsLatitudeDataNb = JB.addData("Latitude","");
  jetiGpsLongitudeDataNb = JB.addData("Longitude","");
  jetiGpsAltitudeDataNb = JB.addData("Altitude","m");
  JB.setValue(JB.addData("Nb sat","sat"),&jetiNumSats);
  JB.setValue(JB.addData("Boussole","deg"),&jetiBoussole);
  JB.setValue(JB.addData("Hauteur","m"),&jetiHauteur);
  JB.setValue(JB.addData("Voltage","V"),&jetiVoltage,2);
  JB.setValue(JB.addData("Courant","A"),&jetiCourant,1);

  JB.setValue(JB.addData("Puissance","W"),&jetiPuissance,1);
  //  JB.setValueDate(JB.addData("Date",""),jetiDate);
  //  JB.setValueTime(JB.addData("Time",""),jetiTime);
  JB.setValue(JB.addData("WP Dist","m"),&jetiWpDistance);
  jetiWaypointDataNb =JB.addData("WayPoint","wp");

  jetiLaseControlMode = control_mode;

}

void userhook_50Hz()
{
  // put your 50Hz code here

  if (!lastMotorArmed&&motors.armed()) {
    JB.alarm('F');
    beforeRepeatAlarm=1;
  }
  if (beforeRepeatAlarm) {
    beforeRepeatAlarm--;
  } 
  else {
    if ((6<battery_voltage1)&&(battery_voltage1<(3.4*3))&&lastMotorArmed) {
      JB.alarm('Z');
      beforeRepeatAlarm=250;
    }
  }
  lastMotorArmed=motors.armed();

  jetiSelect--;

  if ((jetiLastGpsStatus != GPS::GPS_OK)&&(g_gps->status() == GPS::GPS_OK)) {
    JB.setValueGPS(jetiGpsLatitudeDataNb,jetiGpsLatitude);
    JB.setValueGPS(jetiGpsLongitudeDataNb,jetiGpsLongitude);
    JB.setValue(jetiGpsAltitudeDataNb,&jetiAltitude);
  }
  if ((jetiLastGpsStatus == GPS::GPS_OK)&&(g_gps->status() != GPS::GPS_OK)) {
    JB.unsetValue(jetiGpsLatitudeDataNb);
    JB.unsetValue(jetiGpsLongitudeDataNb);
    JB.unsetValue(jetiGpsAltitudeDataNb);
  }

  if (control_mode == AUTO) {
    if (jetiLaseControlMode != AUTO) {
      JB.setValue(jetiWaypointDataNb,&jetiWaypointNum);
    }
  } 
  else {
    JB.unsetValue(jetiWaypointDataNb);
  }
  jetiLaseControlMode = control_mode;

  jetiLastGpsStatus=g_gps->status();

  if (jetiSelect==0) {
    jetiSelect=3;

    jetiGpsLatitude[1]=g_gps->latitude/10000000;
    gpsMinute = (g_gps->latitude%10000000)*6/1000 ;
    jetiGpsLatitude[2]=(gpsMinute&0xff00)>>8;
    jetiGpsLatitude[3]=gpsMinute&0x00ff;
    jetiGpsLongitude[1]=g_gps->longitude/10000000;
    gpsMinute = (g_gps->longitude%10000000)*6/1000 ;
    jetiGpsLongitude[2]=(gpsMinute&0xff00)>>8;
    jetiGpsLongitude[3]=gpsMinute&0x00ff;
    jetiAltitude = g_gps->altitude/100;
    jetiNumSats = g_gps->num_sats;
    jetiBoussoleHeading = compass.calculate_heading(ahrs.get_dcm_matrix());
    jetiBoussole = (wrap_360(ToDeg(jetiBoussoleHeading) * 100)) /100;
    jetiHauteur = (current_loc.alt - home.alt)/100;
    jetiVoltage = battery_voltage1 ;
    jetiCourant = current_amps1 ;
    jetiPuissance = jetiVoltage * jetiCourant ;
    jetiWpDistance = wp_distance / 100;
    jetiWaypointNum = g.command_index;

    jetiButton = JB.readbuttons();
    switch (jetiMenu) {
    case JETI_START:
      if (jetiButton==JB_key_right) {
        jetiMenu = JETI_STB_MEMORY;
      }
      if (jetiButton==JB_key_down) {
        jetiMenu = JETI_GPS1;
      }
      break;
    case JETI_GPS1:
    case JETI_GPS2:
    case JETI_BATTERY:
      switch (jetiButton) {
      case JB_key_up:
        jetiMenu--;
        break;
      case JB_key_down:
        jetiMenu++;
        break;
      }
      break;
    case JETI_TEST:
      if (jetiButton==JB_key_up) {
        jetiMenu = JETI_BATTERY;
      }
      if (jetiButton==JB_key_right) {
        testChar++;
      }
      if (jetiButton==JB_key_left) {
        testChar--;
      }
      break;
    case JETI_STB_MEMORY:
      if (jetiButton==JB_key_up) {
        jetiMenu = JETI_START;
      } 
      else if (jetiButton==JB_key_down) {
        jetiMenu++;
      } 
      else if (jetiButton==(JB_key_right|JB_key_left)) {
        g.pi_stabilize_roll.save_gains();
        g.pi_stabilize_pitch.save_gains();
        g.pi_stabilize_yaw.save_gains();
      }
      else if (jetiButton==JB_key_right) {
        jetiMenu = JETI_RATE_MEMORY;
      } 
      break;
    case JETI_RATE_MEMORY:
      switch (jetiButton) {
      case JB_key_up:
        jetiMenu = JETI_START;
        break;
      case JB_key_down:
        jetiMenu++;
        break;
      case JB_key_right|JB_key_left:
        g.pid_rate_roll.save_gains();
        g.pid_rate_pitch.save_gains();
        g.pid_rate_yaw.save_gains();
        break;
      case JB_key_left:
        jetiMenu = JETI_STB_MEMORY;
        break;
      case JB_key_right:
        jetiMenu = JETI_ALT_HOLD_MEMORY;
        break;
      }
      break;
    case JETI_ALT_HOLD_MEMORY:
      switch (jetiButton) {
      case JB_key_up:
        jetiMenu = JETI_START;
        break;
      case JB_key_down:
        jetiMenu++;
        break;
      case JB_key_left:
        jetiMenu = JETI_RATE_MEMORY;
        break;
      case JB_key_right:
        jetiMenu = JETI_LOITER_MEMORY;
        break;
      case JB_key_left|JB_key_right:
        g.pi_alt_hold.save_gains();
        g.pid_throttle.save_gains();
        break;
      }
      break;
    case JETI_LOITER_MEMORY:
      switch (jetiButton) {
      case JB_key_up:
        jetiMenu = JETI_START;
        break;
      case JB_key_down:
        jetiMenu++;
        break;
      case JB_key_left:
        jetiMenu = JETI_ALT_HOLD_MEMORY;
        break;
      case JB_key_right:
        jetiMenu = JETI_NAV_MEMORY;
        break;
      case JB_key_left|JB_key_right:
        g.pi_loiter_lat.save_gains();
        g.pi_loiter_lon.save_gains();
        g.pid_loiter_rate_lat.save_gains();
        g.pid_loiter_rate_lon.save_gains();
        break;
      }
      break;
    case JETI_NAV_MEMORY:
      switch (jetiButton) {
      case JB_key_up:
        jetiMenu = JETI_START;
        break;
      case JB_key_down:
        jetiMenu++;
        break;
      case JB_key_left:
        jetiMenu = JETI_LOITER_MEMORY;
        break;
      case JB_key_right:
        jetiMenu = JETI_GIMBAL_MEMORY;
        break;
      case JB_key_left|JB_key_right:
        g.pid_nav_lat.save_gains();
        g.pid_nav_lon.save_gains();
        break;
      }
      break;
    case JETI_GIMBAL_MEMORY:
      switch (jetiButton) {
      case JB_key_up:
        jetiMenu = JETI_START;
        break;
      case JB_key_down:
        jetiMenu++;
        break;
      case JB_key_left:
        jetiMenu = JETI_NAV_MEMORY;
        break;
        //      case JB_key_right:
        //        jetiMenu = JETI_GIMBAL_MEMORY;
        //        break;
      case JB_key_left|JB_key_right:
        break;
      }
      break;
    default:
      strncpy_P(jetiBufferMsg, jetiApParamName[jetiMenu-10], 0x10);
      jetiAp = AP_Param::find(jetiBufferMsg,&jetiApType);
      if (jetiAp != NULL) {
        switch (jetiButton) {
        case JB_key_up:
          jetiMenu--;
          break;
        case JB_key_down:
          if (jetiMenu!=JETI_LAST_PARAM) {
            jetiMenu++;
          }
          break;
        case JB_key_left:
          switch (jetiApType) {
          case AP_PARAM_INT8:
            ((AP_Int8 *)jetiAp)->set(((AP_Int8 *)jetiAp)->get()-1);
            break;
          case AP_PARAM_INT16:
            ((AP_Int16 *)jetiAp)->set(((AP_Int16 *)jetiAp)->get()-1);
            break;
          case AP_PARAM_INT32:
            ((AP_Int32 *)jetiAp)->set(((AP_Int32 *)jetiAp)->get()-1);
            break;
          case AP_PARAM_FLOAT:
            ((AP_Float *)jetiAp)->set(((AP_Float *)jetiAp)->get()-0.001);
            break;
          }
          break;
        case JB_key_right:
          switch (jetiApType) {
          case AP_PARAM_INT8:
            ((AP_Int8 *)jetiAp)->set(((AP_Int8 *)jetiAp)->get()+1);
            break;
          case AP_PARAM_INT16:
            ((AP_Int16 *)jetiAp)->set(((AP_Int16 *)jetiAp)->get()+1);
            break;
          case AP_PARAM_INT32:
            ((AP_Int32 *)jetiAp)->set(((AP_Int32 *)jetiAp)->get()+1);
            break;
          case AP_PARAM_FLOAT:
            ((AP_Float *)jetiAp)->set(((AP_Float *)jetiAp)->get()+0.001);
            break;
          }
          break;
        }
      }
      break;
    }

    if (jetiButton != JB_key_none) {
      switch (jetiMenu) {
      case JETI_START:
        JB.JetiBox(THISFIRMWARE,"     STATUS");
        break;
      case JETI_GPS1:
        sprintf(jetiBufferMsg, "GPS> SAT: %d", g_gps->num_sats);
        if (g_gps->status() == 2) {
          sprintf(jetiBufferMsg2, "Alt: %dm",(uint16_t)(g_gps->altitude/100));
          JB.JetiBox(jetiBufferMsg,jetiBufferMsg2);
        } 
        else {
          JB.JetiBox(jetiBufferMsg,"no FIX");
        }
        break;
      case JETI_GPS2:
        if (g_gps->status() == GPS::GPS_OK) {
          gpsMinute = (g_gps->latitude%10000000)*6/1000 ;
          stringBuffer1 = String("Lat " + String(g_gps->latitude/10000000) + "\xdf" + String(gpsMinute/1000) + "." + String(gpsMinute%1000));
          stringBuffer1.toCharArray(jetiBufferMsg,16);
          gpsMinute = (g_gps->longitude%10000000)*6/1000 ;
          stringBuffer1 = String("Lat " + String(g_gps->longitude/10000000) + "\xdf" + String(gpsMinute/1000) + "." + String(gpsMinute%1000));
          stringBuffer1.toCharArray(jetiBufferMsg2,16);
          JB.JetiBox(jetiBufferMsg,jetiBufferMsg2);
        } 
        else {
          JB.JetiBox("no Data available","");
        }
        break;
      case JETI_BATTERY:
        sprintf(jetiBufferMsg,"V:  ");
        dtostrf(jetiVoltage,1,3, &jetiBufferMsg[4]);
        sprintf(jetiBufferMsg2,"A:  ");
        dtostrf(jetiCourant,1,3, &jetiBufferMsg2[4]);
        JB.JetiBox(jetiBufferMsg,jetiBufferMsg2);
        break;
      case JETI_TEST:
        sprintf(jetiBufferMsg, "TEST: %2x", testChar);
        jetiBufferMsg2[0]=testChar;
        jetiBufferMsg2[1]=0;
        JB.JetiBox(jetiBufferMsg,jetiBufferMsg2);
        break;
      case JETI_STB_MEMORY:
      case JETI_RATE_MEMORY:
      case JETI_ALT_HOLD_MEMORY:
      case JETI_LOITER_MEMORY:
      case JETI_NAV_MEMORY:
      case JETI_GIMBAL_MEMORY:
        strncpy_P(jetiBufferMsg, jetiApParamName[jetiMenu-10], 0x10);
        jetiAp = AP_Param::find(jetiBufferMsg,&jetiApType);
        JB.JetiBox(jetiBufferMsg,"    Save  \x7f\x7e");
        break;
      default:
        strncpy_P(jetiBufferMsg, jetiApParamName[jetiMenu-10], 0x10);
        jetiAp = AP_Param::find(jetiBufferMsg,&jetiApType);
        if (jetiAp == NULL) {
          sprintf(jetiBufferMsg,"%s","Not found");
        } 
        else {
          switch (jetiApType) {
          case AP_PARAM_INT8:
            sprintf(jetiBufferMsg2,"%d",(int)((AP_Int8 *)jetiAp)->get());
            break;
          case AP_PARAM_INT16:
            sprintf(jetiBufferMsg2,"%d",(int)((AP_Int16 *)jetiAp)->get());
            break;
          case AP_PARAM_INT32:
            sprintf(jetiBufferMsg2,"%ld",(long)((AP_Int32 *)jetiAp)->get());
            break;
          case AP_PARAM_FLOAT:
            dtostrf(((AP_Float *)jetiAp)->get(),1,3, jetiBufferMsg2);
            break;
          default:
            sprintf(jetiBufferMsg2,"%s","Unknown type");
            break;
          }
          strncpy_P(jetiBufferMsg, jetiApParamName[jetiMenu-10], 0x10);
          JB.JetiBox(jetiBufferMsg,jetiBufferMsg2);
        }
        break;

      }
    }
    if (!JB.createFrame()) {
      jetiSelect=1;
    }
  }
}


